/*
 * SparseMatrix.tcc
 *
 *  Created on: Oct 27, 2010
 *      Author: jagiella
 */

#include <stdlib.h>
#include <stdio.h>

#include "Mathematix.h"

#ifndef NULL
	#define NULL 0
#endif

template <class T> SparseMatrix<T>::SparseMatrix( int dimI, int dimJ)
{
	//fprintf( stderr, "SparseMatrix::newSparseMatrix(%i,%i)\n",dimI, dimJ);

	this->A        = (T**) malloc( dimI * sizeof(T*)); // values
	if( this->A == NULL) memoryAllocationError((char*) "this->A");
	this->JA       = (int**)   malloc( dimI * sizeof(int*));  // column index of values
	if( this->A == NULL) memoryAllocationError((char*) "this->JA");
	this->sizeA    = (int*)    malloc( dimI * sizeof(int));
	if( this->A == NULL) memoryAllocationError((char*) "this->sizeA");
	this->maxSizeA = (int*)    malloc( dimI * sizeof(int));
	if( this->A == NULL) memoryAllocationError((char*) "this->maxSizeA");

	for( int i=0; i<dimI; i++){
		this->A[i]        = (T*) malloc( ROW_EXTENSION_SIZE * sizeof(T)); // values
		if( this->A[i] == NULL) memoryAllocationError((char*) "this->A[i]");
		this->JA[i]       = (int*)   malloc( ROW_EXTENSION_SIZE * sizeof(int));  // column index of values
		if( this->JA[i] == NULL) memoryAllocationError((char*) "this->JA[i]");
		this->sizeA[i]    = 0;
		this->maxSizeA[i] = ROW_EXTENSION_SIZE;
	}

	this->dimI = dimI;
	this->dimJ = dimJ;
}


template <class T> SparseMatrix<T>::~SparseMatrix()
{
	for( int i=0; i<this->dimI; i++){
		free( this->A[i]);
		free( this->JA[i]);
	}

	free( this->A);
	free( this->JA);
	free( this->sizeA);
	free( this->maxSizeA);

	//free( this);
}

template <class T> T SparseMatrix<T>::get( int i, int j)
{
	for( int jj=0; jj<this->sizeA[i]; jj++){
		if( JA[i][jj] == j){
			return A[i][jj];
		}else if( JA[i][jj] > j){
			return 0.;
		}
	}

	return 0.;
}

template <class T> void SparseMatrix<T>::resetRow( int i)
{
	this->sizeA[i] = 0;
}


template <class T> void SparseMatrix<T>::setLast( int i, int j, T value)
{
	if( value == 0.) return;

	int k = this->sizeA[i];
	if( k > 0 && this->JA[i][k-1] >= j){
		fprintf( stderr, "ERROR in SparseMatrix::setLast(): Element cannot be inserted at last position!! \n(i,j) = (%i,%i), sizeA(i) = %i \n", i, j, k);
		exit( 0);
	}

	// allocate memory
	if( this->sizeA[i] == this->maxSizeA[i]){
		this->maxSizeA[i] += ROW_EXTENSION_SIZE;
		this->JA[i] = (int*)   realloc( this->JA[i], this->maxSizeA[i] * sizeof(int));
		if( this->JA[i] == NULL) memoryAllocationError((char*) "this->JA[i]");
			this->A[i]  = (T*) realloc( this->A[i],  this->maxSizeA[i] * sizeof(T));
		if( this->A[i] == NULL) memoryAllocationError((char*) "this->A[i]");
	}

	// add element
	this->JA[i][k] = j;
	this->A[i][k] = value;
	this->sizeA[i] ++;

	return;

}


template <class T> void SparseMatrix<T>::set( int i, int j, T value)
{
	if( value == 0.) return;

	int ji = 0;
	int jj = this->sizeA[i]-1;
	int k  = 0;
	// look for position
	if( this->sizeA[i]>0){
		ji = 0;
		//int ji = 0;
		//jj = this->sizeA[i]-1;
		int x = j;

		do{
			k = (ji + jj) / 2;
			//fprintf( stderr, "i=%i, j=%i, k=%i\n", ji, jj, k);
			if( x > this->JA[i][k])
				ji = k + 1;
			else
				jj = k - 1;
		}while( (this->JA[i][k] != x) && (ji <= jj));
		// jj > ji =>
	}
	//fprintf( stderr, "(i, j) = (%i, %i), ji=%i, jj=%i, k=%i, this->sizeA[%i]=%i\n", i, j, ji, jj, k, i, this->sizeA[i]);

//	if( this->sizeA[i]==0 || this->JA[i][ji] != j /*jj==-1*/){
	if( this->sizeA[i]==0 || (this->JA[i][k] != j && jj<ji)){
		if( jj<ji)
			k = ji;
		//fprintf( stderr, "new element at the beginning (%i, %i)\n", i, k);
		// realloc memory
		if( this->sizeA[i] == this->maxSizeA[i]){
			this->maxSizeA[i] += ROW_EXTENSION_SIZE;
			this->JA[i] = (int*)   realloc( this->JA[i], this->maxSizeA[i] * sizeof(int));
			if( this->JA[i] == NULL) memoryAllocationError((char*) "this->JA[i]");
			this->A[i]  = (T*) realloc( this->A[i],  this->maxSizeA[i] * sizeof(T));
			if( this->A[i] == NULL) memoryAllocationError((char*) "this->A[i]");
		}
		// shift following elements
		for( int jjj=this->sizeA[i]; jjj>k; jjj--){
			this->JA[i][jjj] = this->JA[i][jjj-1];
			this->A[i][jjj] = this->A[i][jjj-1];
		}
		this->JA[i][k] = j;
		this->A[i][k] = value;
		this->sizeA[i] ++;

	}else{
		//if( ji>jj){
			//fprintf( stderr, "replace (%i, %i)\n", i, k);
			A[i][k] = value;
		//}else{
		//	fprintf( stderr, "don't know\n");
		//}
	}
	return;
}


template <class T> void SparseMatrix<T>::add( int i, int j, T value)
{
	if( value == 0) return;

	int ji = 0;
	int jj = this->sizeA[i]-1;
	int k  = 0;
	// look for position
	if( this->sizeA[i]>0){
		ji = 0;
		//int ji = 0;
		//jj = this->sizeA[i]-1;
		int x = j;

		do{
			k = (ji + jj) / 2;
			//fprintf( stderr, "i=%i, j=%i, k=%i\n", ji, jj, k);
			if( x > this->JA[i][k])
				ji = k + 1;
			else
				jj = k - 1;
		}while( (this->JA[i][k] != x) && (ji <= jj));
		// jj > ji =>
	}
	//fprintf( stderr, "(i, j) = (%i, %i), ji=%i, jj=%i, k=%i, this->sizeA[%i]=%i\n", i, j, ji, jj, k, i, this->sizeA[i]);

//	if( this->sizeA[i]==0 || this->JA[i][ji] != j /*jj==-1*/){
	if( this->sizeA[i]==0 || (this->JA[i][k] != j && jj<ji)){
		if( jj<ji)
			k = ji;
		//fprintf( stderr, "new element at the beginning (%i, %i)\n", i, k);
		// realloc memory
		if( this->sizeA[i] == this->maxSizeA[i]){
			this->maxSizeA[i] += ROW_EXTENSION_SIZE;
			this->JA[i] = (int*)   realloc( this->JA[i], this->maxSizeA[i] * sizeof(int));
			if( this->JA[i] == NULL) memoryAllocationError((char*) "this->JA[i]");
			this->A[i]  = (float*) realloc( this->A[i],  this->maxSizeA[i] * sizeof(float));
			if( this->A[i] == NULL) memoryAllocationError((char*) "this->A[i]");
		}
		// shift following elements
		for( int jjj=this->sizeA[i]; jjj>k; jjj--){
			this->JA[i][jjj] = this->JA[i][jjj-1];
			this->A[i][jjj] = this->A[i][jjj-1];
		}
		this->JA[i][k] = j;
		this->A[i][k] = value; // set first time
		this->sizeA[i] ++;

	}else{
		//if( ji>jj){
			//fprintf( stderr, "replace (%i, %i)\n", i, k);
			A[i][k] += value; // add to old value
		//}else{
		//	fprintf( stderr, "don't know\n");
		//}
	}
	return;
}


template <class T> void SparseMatrix<T>::sparseMatrixVectorProduct( SparseMatrix<T> *A, T *b, T *x)
{
	//x = A*b
	// (n x m) * (m x p) = (n x p)
	// (i x j) * (j x 1) = (i x 1)
	for( int i=0; i<A->dimI; i++){
		x[i] = 0.;
		for( int jj=0; jj<A->sizeA[i]; jj++){
			int j = A->JA[i][jj];
			x[i] += A->A[i][jj] * b[j];
#if DEBUG > 0
			if( isnan(x[i])){
				fprintf( stderr, "nan occures in sparseMatrixVectorProduct\n");
				//fprintf( stderr, "vector b[%i] = %lf\n", j, b[j]);
				x[i] = 0.;
				for( int jj=0; jj<sA->sizeA[i]; jj++){
					j = sA->JA[i][jj];
					x[i] += sA->A[i][jj] * b[j];
					fprintf( stderr, "matrix A[%i][%i] = %lf, vector b[%i] = %lf, x[%i] = %lf\n", i, j, sA->A[i][jj], j, b[j], i, x[i]);
				}
				exit( 0);
			}
#endif
		}
	}
}

template <class T> void SparseMatrix<T>::vectorSparseMatrixProduct( T *b, SparseMatrix<T> *A, T *x)
{
	//x = b*A
	// (n x m) * (m x p) = (n x p)
	// (1 x i) * (i x j) = (1 x j)
	//int jj = 0;
	for( int j=0; j<A->dimI; j++)
		x[j] = 0.;

	for( int i=0; i<A->dimI; i++){
		for( int jj=0; jj<A->sizeA[i]; jj++){
			int j = A->JA[i][jj];
			x[j] += b[i] * A->A[i][jj];
#if DEBUG > 0
			if( isnan(x[j])){
				fprintf( stderr, "nan occures in vectorsparseMatrixProduct\n");
				fprintf( stderr, "vector b[%i] = %lf\n", i, b[i]);
				for( int jj=0; jj<sA->sizeA[i]; jj++){
					j = sA->JA[i][jj];
					fprintf( stderr, "matrix A[%i][%i] = %lf\n", i, j, sA->A[i][jj]);
				}
				exit( 0);
			}
#endif
		}
	}
}


template <class T> void SparseMatrix<T>::printMatrix( const char* name, const char* format)
{
	int n=this->dimI, m = this->dimJ;
	fprintf( stderr, "\"%s\" = [\n", name);
	for( int i=0; i<n; i++)
	{
		for( int j=0; j<m; j++)
		{
			fprintf( stderr, format, this->get(i,j));
		}
		fprintf( stderr, "\n");
	}
	fprintf( stderr, "]\n");
}


template <class T> int SparseMatrix<T>::columns()
{
	return this->dimJ;
}


template <class T> int SparseMatrix<T>::rows()
{
	return this->dimI;
}

template <class T> T SparseMatrix<T>::operator() ( int i, int j)
{
	return this->get( i, j);
}

template <class T> void SparseMatrix<T>::operator() ( int i, int j, T value)
{
	this->set( i, j, value);
}

template <class T> SparseMatrix<T>* SparseMatrix<T>::copy()
{
	SparseMatrix<T>* copy = new SparseMatrix<T>(this->dimI, this->dimJ);
	for( int i=0; i<this->dimI; i++)
		for( int jj=0; jj<this->sizeA[i]; jj++){
			int j = this->JA[i][jj];
			copy->setLast( i, j, this->A[i][jj]);
		}

	return copy;
}

/*template <class T> void SparseMatrix<T>::operator = ( SparseMatrix<T>* A)
{
	for( int i=0; i<A->dimI; i++){
		this->resetRow(i);
		for( int jj=0; jj<A->sizeA[i]; jj++){
			int j = A->JA[i][jj];
			this->setLast( i, j, A->A[i][jj]);
		}
	}
}*/
